import queue
import logging
import threading
import time
from datetime import datetime

from algorithm.yolov5 import YOLOV5Lite_ONNX, plot_one_box

class Planner:
    def __init__(self):
        self.run = False
        self.__log_init()


    def __log_init(self):
        self.logger = logging.getLogger("Planner")

    def open(self,
            model_path,
            id_list,
            thred_cond,
            v_base=0.3,
            r_base=90
            ):
        self.logger.trace('open.enter')
        
        self.model = YOLOV5Lite_ONNX(model_path, id_list)
        self.thred_cond = thred_cond
        self.v_base = v_base
        self.r_base = r_base

        self.logger.trace('open.done')
        return True


    def close(self):
        self.logger.trace('close.enter')

        # TODO:任务清理

        self.logger.trace('close.done')


    def thread(self):
        self.logger.trace('thread.enter')
        # 标志位
        if_return = False
        current_status = "wait digit"
        history_status = []
        history_digit = []
        history_index = 0
        target_room = 0
        cross_time = 0

        def decode(data):
            # 解码数据
            # TODO: 不管原来的data格式是什么
            # 最后都返回的格式为：
            # (
            #  [7,1,3],
            #  {'1': [100,200,300,400], 
            #   '3': [150,250,350,450], 
            #   '7': [0  ,100,50 ,150])
            # ) 
            # 注: 返回的结果，data[0]对应的位置在真实世界中是从左到右的顺序
            # 要求的顺序是从左到右，所以需要先做一个排序，重构代码之后我这里的输出是一个成员为字典的列表
            # 首先我们就要加一个X的中心才能排序
            box_dict = {}
            for i in range(len(data)):
                # 添加一个中心点
                data[i]['center_x'] = (data[i]['box']['x1'] + data[i]['box']['x2']) / 2.0

            data = sorted(data,key=lambda x:x['center_x'])
            numbers = []
            for obj in data:
                numbers.append(int(obj['class_name']))
                box = obj['box']
                box_dict[obj['class_name']] = [box['x1'],box['y1'],box['x2'],box['y2']]
            return [numbers,box_dict]


        while self.run:
            # 是向目标房间运动
            if not if_return:
                # self.logger.info(f'current_status:{current_status}, cross_time:{cross_time}')
                pred_result = None
                digit_data = None

                # 等待"数字识别"结果
                if current_status == "wait digit":
                    # 识别数字
                    try:
                        frame, frame_number = self.inqueue1.get(timeout=0.1)
                    except queue.Empty:
                        # self.logger.spam("No digit data received, waiting...")
                        continue
                    except Exception as e:
                        self.logger.error(f"Error getting digit data: {e}")
                        continue
                    pred_result = self.model.predict(frame, thred_cond=self.thred_cond)
                    if pred_result != []:
                        digit_data = decode(pred_result)
                        self.logger.info(f'Predicted digit data: {digit_data}')
                    else:
                        # self.logger.spam("No digit data detected.")
                        continue

                    # 如果有好几个结果，识别有问题
                    if len(digit_data[0]) != 1:
                        self.logger.warning("multi digit detected")
                        continue
                    else:
                        digit = digit_data[0][0]
                        self.logger.info(f"Digit recognized: {digit}")
                        target_room = digit

                        # 状态转化
                        current_status = "wait media get"
                        continue

                if current_status == "wait media get":
                    try:
                        task = {
                            "task": "GET MEDIA",
                        }
                        self.outqueue1.put([task, 0], timeout=0.1)
                        self.logger.verbose(f"put task {task} to output queue")
                    except Exception as e:
                        # self.logger.spam(f"Error putting task to output queue: {e}")
                        continue
                    try: 
                        if_alive = self.inqueue5.get(timeout=0.1)
                        # self.logger.spam(f"get object alive status {if_alive} from input queue")
                    except Exception as e:
                        # self.logger.spam("input queue is empty, waiting for next object alive status...")
                        continue
                    # 如果物体存在
                    if if_alive[0] == 0:
                        current_status = "trace2cross"
                    

                if current_status == "trace2cross":
                    start_time = time.time()
                    try:
                        signal, signal_ratio = self.inqueue2.get(timeout=0.1)
                    except queue.Empty:
                        # self.logger.spam("No crossing signal data received, waiting...")
                        continue
                    except Exception as e:
                        self.logger.error(f"Error getting crossing signal data: {e}")
                        continue
                    self.logger.info(f"signal detect cost: {time.time()-start_time}")
                    # 没识别到十字
                    if signal[0] == False:
                        start_time = time.time()
                        try:
                            bias, trace_number = self.inqueue3.get(timeout=0.1)
                        except queue.Empty:
                            # self.logger.spam("No digit data received, waiting...")
                            continue
                        except Exception as e:
                            self.logger.error(f"Error getting digit data: {e}")
                            continue
                        # 发送巡线指令
                        task = {
                            "task": "trace",
                            "bias": bias
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, trace_number], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                        self.logger.info(f"trace cost: {time.time()-start_time}")
                        continue

                    # 识别到十字
                    else:
                        if cross_time == 0:
                            # 第一个分岔口
                            if target_room not in [1, 2]:
                                # 状态转化
                                history_status.append("forward")
                                cross_time = 1
                                # 开环走
                                task = {
                                    "task": "forward",
                                    "distance": 0.4
                                }
                                try:
                                    if self.outqueue1.full():
                                        self.outqueue1.get_nowait()
                                    self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                                except Exception as e:
                                    self.logger.error(f"Error sending task to output queue: {e}")
                                    continue
                                time.sleep(task["distance"] / self.v_base)  # 假设前进速度为0.2m/s
                                continue

                            else:
                                task = {
                                    "task": "forward",
                                    "distance": 0.15
                                }
                                try:
                                    if self.outqueue1.full():
                                        self.outqueue1.get_nowait()
                                    self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                                except Exception as e:
                                    self.logger.error(f"Error sending task to output queue: {e}")
                                    continue
                                time.sleep(task["distance"] / self.v_base)  # 假设前进速度为0.2m/s


                                if target_room == 1:
                                    task = {
                                        "task": "left",
                                    }
                                else:
                                    task = {
                                        "task": "right",
                                    }
                                try:
                                    if self.outqueue1.full():
                                        self.outqueue1.get_nowait()
                                    self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                                    time.sleep(90/self.r_base)  # 假设转向速度为90°/s
                                except Exception as e:
                                    self.logger.error(f"Error sending task to output queue: {e}")
                                    continue
                                # 状态记录
                                history_status.append(task["task"])
                                # 状态转化
                                current_status = "trace2door"
                                continue


                        elif cross_time in [1, 3]:
                            # 第二个分岔口
                            # 先停下
                            task = {
                                "task": "wait",
                            }
                            try:
                                if self.outqueue1.full():
                                    self.outqueue1.get_nowait()
                                self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                            except Exception as e:
                                self.logger.error(f"Error sending task to output queue: {e}")
                                continue
                            # 识别数字
                            try:
                                frame, frame_number = self.inqueue1.get(timeout=0.1)
                            except queue.Empty:
                                # self.logger.spam("No digit data received, waiting...")
                                continue
                            except Exception as e:
                                self.logger.error(f"Error getting digit data: {e}")
                                continue
                            pred_result = self.model.predict(frame, thred_cond=self.thred_cond)
                            if pred_result != []:
                                digit_data = decode(pred_result)
                                self.logger.info(f'Predicted digit data: {digit_data}')
                            else:
                                # self.logger.spam("No digit data detected.")
                                continue
                            history_digit.append(digit_data[0])
                            # 如果不在第二个路口的房间
                            if target_room not in digit_data[0] and cross_time == 1:
                                self.logger.error(f"{cross_time}: {digit_data[0]}")
                                # 状态转化
                                history_status.append("forward")
                                cross_time = 2
                                # 继续前进
                                task = {
                                    "task": "forward",
                                    "distance": 0.5
                                }
                                try:
                                    if self.outqueue1.full():
                                        self.outqueue1.get_nowait()
                                    self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                                except Exception as e:
                                    self.logger.error(f"Error sending task to output queue: {e}")
                                    continue
                                time.sleep(task["distance"] / self.v_base)
                                continue

                            else:
                                task = {
                                    "task": "forward",
                                    "distance": 0.15
                                }
                                try:
                                    if self.outqueue1.full():
                                        self.outqueue1.get_nowait()
                                    self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                                except Exception as e:
                                    self.logger.error(f"Error sending task to output queue: {e}")
                                    continue
                                time.sleep(task["distance"] / 0.2)  # 假设前进速度为0.2m/s
                                self.logger.info(f"digit:{digit_data[0]}")
                                if target_room == digit_data[0][0]:
                                    task = {
                                        "task": "left",
                                    }
                                else:
                                    task = {
                                        "task": "right",
                                    }
                                try:
                                    if self.outqueue1.full():
                                        self.outqueue1.get_nowait()
                                    self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                                    time.sleep(90 / self.r_base)  # 假设转向速度为90°/s
                                except Exception as e:
                                    self.logger.error(f"Error sending task to output queue: {e}")
                                    continue
                                # 状态记录
                                history_status.append(task["task"])
                                # 状态转化
                                current_status = "trace2door"
                                continue


                        elif cross_time == 2:
                            """
                            第三个分岔路，这里有四张数字，
                            先右转.如果数字在右边,就转回然后左、右转(左右监测幅度为30°,检测回正后开环直行0.3m)
                            """
                            # 先右转30°
                            task = {
                                "task": "right_30",
                            }
                            try:
                                if self.outqueue1.full():
                                    self.outqueue1.get_nowait()
                                self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                            except Exception as e:
                                self.logger.error(f"Error sending task to output queue: {e}")
                                continue
                            # 先取出当前识别结果(如果有),再重新识别新的视野
                            time.sleep(1)
                            # 等待"数字识别"结果
                            while True:
                                # 识别数字
                                try:
                                    frame, frame_number = self.inqueue1.get(timeout=0.1)
                                except queue.Empty:
                                    # self.logger.spam("No digit data received, waiting...")
                                    continue
                                except Exception as e:
                                    self.logger.error(f"Error getting digit data: {e}")
                                    continue
                                pred_result = self.model.predict(frame, thred_cond=self.thred_cond)
                                if pred_result != []:
                                    digit_data = decode(pred_result)
                                    self.logger.info(f'Predicted digit data: {digit_data}')
                                else:
                                    # self.logger.spam("No digit data detected.")
                                    continue
                                if len(digit_data[0]) == 2:
                                    self.logger.info(f"right Digit recognized: {digit_data[0]}")
                                    break
                            # 转回中轴，前进到路口
                            task = {
                                "task": "left_30",
                            }
                            try:
                                if self.outqueue1.full():
                                    self.outqueue1.get_nowait()
                                self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                            except Exception as e:
                                self.logger.error(f"Error sending task to output queue: {e}")
                                continue
                            time.sleep(30 / self.r_base)  # 假设转向速度为90°/s

                            task = {
                                "task": "forward",
                                "distance": 0.15
                            }
                            try:
                                if self.outqueue1.full():
                                    self.outqueue1.get_nowait()
                                self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                            except Exception as e:
                                self.logger.error(f"Error sending task to output queue: {e}")
                                continue
                            time.sleep(task["distance"] / self.v_base + 0.1)
                            history_digit.append(digit_data[0])
                            # 根据识别结果决定转向
                            if target_room in digit_data[0]:
                                # 如果目标房间在右边
                                task = {
                                    "task": "right",
                                }
                            else:
                                # 如果目标房间在左边
                                task = {
                                    "task": "left",
                                }
                            try:
                                if self.outqueue1.full():
                                    self.outqueue1.get_nowait()
                                self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                                time.sleep(90 / self.r_base)  # 假设转向速度为90°/s
                            except Exception as e:
                                self.logger.error(f"Error sending task to output queue: {e}")
                                continue
                            
                            # 状态记录
                            history_status.append(task["task"])
                            # 状态转化
                            cross_time = 3 
                                
                            

                if current_status == "trace2door":
                    try:
                        signal, signal_ratio = self.inqueue2.get(timeout=0.1)
                    except queue.Empty:
                        # self.logger.spam("No crossing signal data received, waiting...")
                        continue
                    except Exception as e:
                        self.logger.error(f"Error getting crossing signal data: {e}")
                        continue

                    if signal[1] == False:
                        try:
                            bias, trace_number = self.inqueue3.get(timeout=0.1)
                        except queue.Empty:
                            # self.logger.spam("No digit data received, waiting...")
                            continue
                        except Exception as e:
                            self.logger.error(f"Error getting digit data: {e}")
                            continue
                        # 发送巡线指令
                        task = {
                            "task": "trace",
                            "bias": bias
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, trace_number], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                        continue

                    else:
                        # 识别到门
                        # 停止，点红灯，UDP发送到达信号
                        task = {
                            "task": "wait",
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, signal_ratio[1]], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                            continue
                        time.sleep(0.2)
                        self.logger.error("waiting")
                        task = {
                            "task": "RED ON"
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, signal_ratio[1]], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                            continue
                        
                        # udp发送数字
                        task = {
                            "task": "UDP SEND",
                            "info": {
                                "status": "target_room",
                                "target_room": target_room,
                                "digit_stack": history_digit}
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, frame_number], timeout=0.1)
                        except queue.Full:
                            self.logger.error("Output queue is full, unable to send task.")
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")

                        # 状态转化
                        current_status = "wait object release"
                

                if current_status == "wait object release":
                    # 等待下位机返回物体被拿走的信号
                    try:
                        task = {
                            "task": "GET MEDIA",
                        }
                        self.outqueue1.put([task, 0], timeout=0.1)
                        self.logger.verbose(f"put task {task} to output queue")
                    except Exception as e:
                        # self.logger.spam(f"Error putting task to output queue: {e}")
                        continue
                    try: 
                        if_alive = self.inqueue5.get(timeout=0.1)
                        # self.logger.spam(f"get object alive status {if_alive} from input queue")
                    except Exception as e:
                        # self.logger.spam("input queue is empty, waiting for next object alive status...")
                        continue
                    # 如果物体被拿走
                    if if_alive[0] == 1:
                        current_status = "trace2cross"
                    else:
                        continue

                    # 关闭红灯
                    task = {
                        "task": "RED OFF"
                    }
                    try:
                        if self.outqueue1.full():
                            self.outqueue1.get_nowait()
                        self.outqueue1.put([task, 0], timeout=0.1)
                    except Exception as e:
                        self.logger.error(f"Error sending task to output queue: {e}")
                        continue

                    # 自转回头
                    task = {
                        "task": "backward"
                    }
                    try:
                        if self.outqueue1.full():
                            self.outqueue1.get_nowait()
                        self.outqueue1.put([task, 0], timeout=0.1)
                    except Exception as e:
                        self.logger.error(f"Error sending task to output queue: {e}")
                        continue
                    time.sleep(180 / self.r_base)  # 假设转向速度为90°/s
                    time.sleep(0.2)
                    # UDP发送返回信号
                    task = {
                        "task": "UDP SEND",
                        "info": {
                            "status": "command",
                            "command": "car1 return"}
                    }
                    try:
                        if self.outqueue1.full():
                            self.outqueue1.get_nowait()
                        self.outqueue1.put([task, 0], timeout=0.1)
                        self.logger.error("----------------commandsend-------------------")
                    except Exception as e:
                        self.logger.error(f"Error sending task to output queue: {e}")
                    time.sleep(0.2)
                    # 状态转化
                    if_return = True
                    current_status = "trace2cross"
                    history_index = len(history_status) - 1
                    self.logger.error(f"{history_status}")


            # 是返回起始房间
            else:
                self.logger.info(f'current_status:{current_status}, cross_time:{history_index}')
                if current_status == "trace2cross":
                    try:
                        signal, signal_ratio = self.inqueue2.get(timeout=0.1)
                    except queue.Empty:
                        # self.logger.spam("No crossing signal data received, waiting...")
                        continue
                    except Exception as e:
                        self.logger.error(f"Error getting crossing signal data: {e}")
                        continue

                    # 没识别到十字
                    if signal[0] == False:
                        try:
                            bias, trace_number = self.inqueue3.get(timeout=0.1)
                        except queue.Empty:
                            # self.logger.spam("No digit data received, waiting...")
                            continue
                        except Exception as e:
                            self.logger.error(f"Error getting digit data: {e}")
                            continue
                        # 发送巡线指令
                        task = {
                            "task": "trace",
                            "bias": bias
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, trace_number], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                        continue

                    else:
                        # 先到达十字上
                        task = {
                            "task": "forward",
                            "distance": 0.15
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                            continue
                        time.sleep(task["distance"] / self.v_base)  # 假设前进速度为0.2m/s
                        # 根据历史状态决定任务
                        action = history_status[history_index]
                        if action == "left":
                            task = {
                                "task": "right",
                            }
                        elif action == "right":
                            task = {
                                "task": "left",
                            }
                        elif action == "forward":
                            task = {
                                "task": "forward",
                                "distance": 0.4
                            }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, signal_ratio[0]], timeout=0.1)
                            time.sleep(0.4 / self.v_base)  # 假设前进速度为0.2m/s
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                            continue
                        history_index -= 1
                        if history_index <= 0:
                            current_status = "trace2door"

                elif current_status == "trace2door":
                    try:
                        signal, signal_ratio = self.inqueue2.get(timeout=0.1)
                    except queue.Empty:
                        # self.logger.spam("No crossing signal data received, waiting...")
                        continue
                    except Exception as e:
                        self.logger.error(f"Error getting crossing signal data: {e}")
                        continue

                    if signal[1] == False:
                        try:
                            bias, trace_number = self.inqueue3.get(timeout=0.1)
                        except queue.Empty:
                            # self.logger.spam("No digit data received, waiting...")
                            continue
                        except Exception as e:
                            self.logger.error(f"Error getting digit data: {e}")
                            continue
                        # 发送巡线指令
                        task = {
                            "task": "trace",
                            "bias": bias
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, trace_number], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                        continue

                    else:
                        # 识别到门
                        # 停止，点红灯，UDP发送到达信号
                        task = {
                            "task": "wait",
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, signal_ratio[1]], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                            continue
                        time.sleep(0.2)
                        task = {
                            "task": "GREEN ON"
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, signal_ratio[1]], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                            continue
                        # UDP发送到达信号
                        task = {
                            "task": "udp send",
                            "info": {"target_room": target_room, "status": "arrived"}
                        }
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([task, signal_ratio[1]], timeout=0.1)
                        except Exception as e:
                            self.logger.error(f"Error sending task to output queue: {e}")
                            continue
                        # 状态转化
                        current_status = "end"
                if current_status == "end":
                    # 阻塞在这里
                    # while True:
                    self.logger.info("End")
                    time.sleep(1)
        
        self.logger.trace('thread.done')


    def start(self):
        self.logger.trace('start.enter')

        self.run = True
        self.handle = threading.Thread(target=self.thread)
        self.handle.start()

        self.logger.trace('start.done')
        return True


    def stop(self):
        self.logger.trace('stop.enter')
        self.run = False
        self.handle.join()
        self.logger.trace('stop.done')


    def input_queue_set(self, input_queue_list):
        # 设置输入队列，接受的是一个队列列表
        self.logger.trace('input_queue_set.enter')

        if not isinstance(input_queue_list, list):
            input_queue_list = [input_queue_list]

        for input_queue in input_queue_list:
            if not isinstance(input_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.inqueue1 = input_queue_list[0]                     # "数字识别"结果    [digit, number]
        self.inqueue2 = input_queue_list[1]                     # "路口标志检测"    [crossing_signal, cross_ratio]
        self.inqueue3 = input_queue_list[2]                     # "巡线"结果        [bias, number]
        self.inqueue4 = input_queue_list[3]                     # "UPD接收"结果     [data, number]  
        self.inqueue5 = input_queue_list[4]                     # "对象检测"结果    [if_alive]

        self.logger.trace('input_queue_set.done')
        return True


    def output_queue_set(self, output_queue_list):
        # 设置输出队列，接受的是一个队列列表
        self.logger.trace('output_queue_set.enter')

        if not isinstance(output_queue_list, list):
            output_queue_list = [output_queue_list]

        for output_queue in output_queue_list:
            if not isinstance(output_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.outqueue1 = output_queue_list[0]

        self.logger.trace('output_queue_set.done')
        return True
